/* This code is currently configured to create a single multi-scale GADGET-2 initial conditions file
 * from a total of up to five nested GRAFIC initial conditions files: 1 top-grid and up to 4 nested sub-grids.
 * Evan Rule 2013
 * Space Telescope Science Institute
*/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>

// A simple function to find the minimum of two numbers
#define MIN(a,b) ((((a)-(b))&0x80000000)>>31)? (a):(b)

// Here, NMULT is the number of output files which will be written for EACH grid level
// Please choose NMULT so that np3 for each level is divisible by NMULT
#define NMULT 4

//Here we define prototypes of funcions that will be used to calculate the Zeldovich approximation
double dplus(float, float, float);
float fomega(float, float, float);
double ddplus(float, float, float);
double rombint(double (*f)(float, float, float), double a, double b, double tol, float omegam, float omegav);
float dladt(float, float, float);

int main(int argc, char *argv[])
{
	// Initialize variables
	int np1a, np2a, np3a;
	int np1b, np2b, np3b;
	float dxa, x1oa, x2oa, x3oa;
	float dxb, x1ob, x2ob, x3ob;
	float dx, np; 
	float astarta, omegama, omegava, h0a;
	float astartb, omegamb, omegavb, h0b;
	float astart, omegam, omegav, h0;
	float x0, y0, z0;
	float xdim, ydim, zdim;
	float xLbox_outb;
	int i, k, i1, i2, i3, ifile;
	int junk;
	float vfact;
	int inbox = 0;
	int inboxtotal = 0;
	int prev = 0;
	int prevtotal = 0;
	double a_out, redshift_out;
	int flag_sfr_out, flag_feedback_out, flag_cooling_out, numfiles_out;
	double xLbox_out, omega0_out, omegaL_out, hubble_out;

	// Initialize arrays
	float *velx, *vely, *velz, *velx2, *vely2, *velz2;
	unsigned *npart_out, *nall_out;
	double *massarr_out;
	int *unused_out;
	
	// Initialize input file paths
	FILE *file1;
	FILE *file2;
	FILE *file3;
	FILE *file4;
	FILE *file5;
	FILE *file6;
	FILE *current_file;

	char inpath1[100];
	char inpath2[100];
	char inpath3[100];
	char inpath4[100];
	char inpath5[100];
	char inpath6[100];

	// Initialize headers
	int header_size = 256;
	int vel_size;
	int pos_size;
	int id_size;
	int pid;

	// Initialize arrays
	massarr_out = malloc(6*sizeof(double));
	nall_out = malloc(6*sizeof(unsigned int));
	npart_out = malloc(6*sizeof(unsigned int));
	unused_out = malloc(24*sizeof(int));

	// Initialize several arrays to zero 
	for (i = 0; i < 24; i++)
	{
		unused_out[i] = 0;
	}

	for (i = 0; i < 6; i++)
	{
		massarr_out[i] = 0;
		nall_out[i] = 0;
		npart_out[i] = 0;
	}

	// Begin loop over grid levels	
	for (k = 0; k < argc - 3; k++)
	{	
		// Set the correct input paths
		sprintf(inpath1, "%s", argv[k + 1]);
		sprintf(inpath2, "%s", argv[k + 1]);
		sprintf(inpath3, "%s", argv[k + 1]);
		sprintf(inpath4, "%s", argv[k + 2]);
		sprintf(inpath5, "%s", argv[k + 2]);
		sprintf(inpath6, "%s", argv[k + 2]);
	
		// Read header data from each file
		file1 = fopen(strcat(inpath1, "ic_velcx"), "r+b");
		fread(&junk, sizeof(int), 1, file1);
		fread(&np1a, sizeof(int), 1, file1);
		fread(&np2a, sizeof(int), 1, file1);
		fread(&np3a, sizeof(int), 1, file1);
		fread(&dxa, sizeof(float), 1, file1);
		fread(&x1oa, sizeof(float), 1, file1);
		fread(&x2oa, sizeof(float), 1, file1);
		fread(&x3oa, sizeof(float), 1, file1);
		fread(&astarta, sizeof(float), 1, file1);
		fread(&omegama, sizeof(float), 1, file1);
		fread(&omegava, sizeof(float), 1, file1);
		fread(&h0a, sizeof(float), 1, file1);
	
		file4 = fopen(strcat(inpath4, "ic_velcx"), "r+b");
		fread(&junk, sizeof(int), 1, file4);
		fread(&np1b, sizeof(int), 1, file4);
		fread(&np2b, sizeof(int), 1, file4);
		fread(&np3b, sizeof(int), 1, file4);
		fread(&dxb, sizeof(float), 1, file4);
		fread(&x1ob, sizeof(float), 1, file4);
		fread(&x2ob, sizeof(float), 1, file4);
		fread(&x3ob, sizeof(float), 1, file4);
		fread(&astartb, sizeof(float), 1, file4);
		fread(&omegamb, sizeof(float), 1, file4);
		fread(&omegavb, sizeof(float), 1, file4);
		fread(&h0b, sizeof(float), 1, file4);
		
		printf("\n");	
		printf("********* Level %d *********** \n", k);
		printf("np1 = %d \n", np1a);
		printf("np2 = %d \n", np2a);
		printf("np3 = %d \n", np3a);
		printf("omegam = %f \n", omegama);
		printf("omegav = %f \n", omegava);
		printf("h0 = %f \n", h0a);
		printf("zstart = %f \n", 1.0/astarta - 1.0);
		printf("Grid spacing (Mpc) = %lf \n", dxa);
		printf("Grid offsets (Mpc) = %lf %lf %lf \n", x1oa, x2oa, x3oa);
		
		printf("\n");

		printf("********* Level %d *********** \n", k+1);
		printf("np1 = %d \n", np1b);
		printf("np2 = %d \n", np2b);
		printf("np3 = %d \n", np3b);
		printf("omegam = %f \n", omegamb);
		printf("omegav = %f \n", omegavb);
		printf("h0 = %f \n", h0b);
		printf("zstart = %f \n", 1.0/astartb - 1.0);
		printf("Grid spacing (Mpc) = %lf \n", dxb);
		printf("Grid offsets (Mpc) = %lf %lf %lf \n", x1ob, x2ob, x3ob);
		printf("\n");
	
		// Check that the cosmologies are the same between grid levels
		if ((omegama != omegamb) || (omegava != omegavb) || (h0a != h0b) || (astarta != astartb))
		{
			printf("Error: Level %d and level %d cosmologies are not identical \n", k, k + 1);
			exit(EXIT_FAILURE);
		}
	
		// Consolidate relevant cosmology parameters	
		omegam = omegama;
		omegav = omegava;
		h0 = h0a;
		astart = astarta;
		
		// Set mass of grid particles
		massarr_out[k + 1] = omegam*2.7755*10*pow(dxa*np1a*h0/100.0, 3)/(np1a*np2a*np3a);
		massarr_out[k + 2] = omegam*2.7755*10*pow(dxb*np1b*h0/100.0, 3)/(np1b*np2b*np3b);	
		
		// Read header data from remaining input files
		file2 = fopen(strcat(inpath2, "ic_velcy"), "r+b");
		fread(&junk, sizeof(int), 1, file2);
		fread(&np1a, sizeof(int), 1, file2);
		fread(&np2a, sizeof(int), 1, file2);
		fread(&np3a, sizeof(int), 1, file2);
		fread(&dxa, sizeof(float), 1, file2);
		fread(&x1oa, sizeof(float), 1, file2);
		fread(&x2oa, sizeof(float), 1, file2);
		fread(&x3oa, sizeof(float), 1, file2);
		fread(&astarta, sizeof(float), 1, file2);
		fread(&omegama, sizeof(float), 1, file2);
		fread(&omegava, sizeof(float), 1, file2);
		fread(&h0a, sizeof(float), 1, file2);

		file3 = fopen(strcat(inpath3, "ic_velcz"), "r+b");
		fread(&junk, sizeof(int), 1, file3);
		fread(&np1a, sizeof(int), 1, file3);
		fread(&np2a, sizeof(int), 1, file3);
		fread(&np3a, sizeof(int), 1, file3);
		fread(&dxa, sizeof(float), 1, file3);
		fread(&x1oa, sizeof(float), 1, file3);
		fread(&x2oa, sizeof(float), 1, file3);
		fread(&x3oa, sizeof(float), 1, file3);
		fread(&astarta, sizeof(float), 1, file3);
		fread(&omegama, sizeof(float), 1, file3);
		fread(&omegava, sizeof(float), 1, file3);
		fread(&h0a, sizeof(float), 1, file3);	

		file5 = fopen(strcat(inpath5, "ic_velcy"), "r+b");
		fread(&junk, sizeof(int), 1, file5);
		fread(&np1b, sizeof(int), 1, file5);
		fread(&np2b, sizeof(int), 1, file5);
		fread(&np3b, sizeof(int), 1, file5);
		fread(&dxb, sizeof(float), 1, file5);
		fread(&x1ob, sizeof(float), 1, file5);
		fread(&x2ob, sizeof(float), 1, file5);
		fread(&x3ob, sizeof(float), 1, file5);
		fread(&astartb, sizeof(float), 1, file5);
		fread(&omegamb, sizeof(float), 1, file5);
		fread(&omegavb, sizeof(float), 1, file5);
		fread(&h0b, sizeof(float), 1, file5);

		file6 = fopen(strcat(inpath6, "ic_velcz"), "r+b");
		fread(&junk, sizeof(int), 1, file6);
		fread(&np1b, sizeof(int), 1, file6);
		fread(&np2b, sizeof(int), 1, file6);
		fread(&np3b, sizeof(int), 1, file6);
		fread(&dxb, sizeof(float), 1, file6);
		fread(&x1ob, sizeof(float), 1, file6);
		fread(&x2ob, sizeof(float), 1, file6);
		fread(&x3ob, sizeof(float), 1, file6);
		fread(&astartb, sizeof(float), 1, file6);
		fread(&omegamb, sizeof(float), 1, file6);
		fread(&omegavb, sizeof(float), 1, file6);
		fread(&h0b, sizeof(float), 1, file6);	
		
		// Set output header data
		a_out = astart;
		redshift_out = 1.0/a_out - 1.0;
		flag_sfr_out = 0;
		flag_feedback_out = 0;
		flag_cooling_out = 0;
		numfiles_out = 1;
		hubble_out = (double)h0/100.0;
		xLbox_out = 1000.0*dxa*(double)np1a*hubble_out;
		omega0_out = omegam;
		omegaL_out = omegav;
	
		printf("Box size (Kpc/h) = %lf \n", xLbox_out);
	
		// Calculate the dimensions of the subgrid
		xdim = dxb*(double)np1b;
		ydim = dxb*(double)np2b;
		zdim = dxb*(double)np3b;

		// Finish reading header files
		fread(&junk, sizeof(int), 1, file1);
		fread(&junk, sizeof(int), 1, file1);
		fread(&junk, sizeof(int), 1, file2);
		fread(&junk, sizeof(int), 1, file2);
		fread(&junk, sizeof(int), 1, file3);
		fread(&junk, sizeof(int), 1, file3);
		fread(&junk, sizeof(int), 1, file4);
		fread(&junk, sizeof(int), 1, file4);
		fread(&junk, sizeof(int), 1, file5);
		fread(&junk, sizeof(int), 1, file5);
		fread(&junk, sizeof(int), 1, file6);
		fread(&junk, sizeof(int), 1, file6);

		// Calculate Zeldovich approximation
		vfact = fomega(astart, omegam, omegav)*h0*dladt(astart, omegam, omegav)/astart;

		// Here, we read in the velocity data, apply the Zeldovich approximation to find positions
		// and then check how many top grid particles lie within the subgrid
		// The reason for this redundancy is that the first output file header
		// needs to know how many particles there are in total, but unless we check
		// beforehand how many particles are going to be removed, then the first file would
		// not know this information at the time of writing its header.

		printf("\n");
		printf("Counting particles to be removed from high res. region of level %d \n", k);
		inbox = 0;

		for (ifile = 0; ifile < NMULT; ifile++)
		{
			// Initialize arrays
			velx = malloc(np1a*np2a*np3a*sizeof(float)/NMULT);
			if (!velx){printf("Memory allocation failed \n");}
			vely = malloc(np1a*np2a*np3a*sizeof(float)/NMULT);
			if (!vely){printf("Memory allocation failed \n");}
			velz = malloc(np1a*np2a*np3a*sizeof(float)/NMULT);
			if (!velz){printf("Memory allocation failed \n");}

			for (i3 = 0; i3 < np3a/NMULT; i3++)
			{
				z0 = (i3 + ifile*np3a/NMULT)*dxa + x3oa;
				
				for (i2 = 0; i2 < np2a; i2++)
				{
					y0 = i2*dxa + x2oa;
					
					for (i1 = 0; i1 < np1a; i1++)
					{
						x0 = i1*dxa + x1oa;
		
						//Read velocity data from input files
						fread(&velx[i1 + np2a*(i2 + np3a*i3)], sizeof(float), 1, file1);
						fread(&vely[i1 + np2a*(i2 + np3a*i3)], sizeof(float), 1, file2);
						fread(&velz[i1 + np2a*(i2 + np3a*i3)], sizeof(float), 1, file3);

						
							
						velx[i1 + np2a*(i2 + np3a*i3)] = x0 + velx[i1 + np2a*(i2 + np3a*i3)]/vfact;
						vely[i1 + np2a*(i2 + np3a*i3)] = y0 + vely[i1 + np2a*(i2 + np3a*i3)]/vfact;	
						velz[i1 + np2a*(i2 + np3a*i3)] = z0 + velz[i1 + np2a*(i2 + np3a*i3)]/vfact;
						
						if ((velx[i1 + np2a*(i2 + np3a*i3)] > x1ob) && (velx[i1+np2a*(i2 + np3a*i3)] < x1ob + xdim) &&
						    (vely[i1 + np2a*(i2 + np3a*i3)] > x2ob) && (vely[i1+np2a*(i2 + np3a*i3)] < x2ob + ydim) &&
						    (velz[i1 + np2a*(i2 + np3a*i3)] > x3ob) && (velz[i1+np2a*(i2 + np3a*i3)] < x3ob + zdim))
						{
							inbox = inbox + 1;
							inboxtotal = inboxtotal + 1;
						}
					}
				}
			}
		}
		
		printf("%u particles will be removed from level %d grid \n", inbox, k);
		nall_out[k+1] = np1a*np2a*np3a - inbox;
		nall_out[k+2] = np1b*np2b*np3b;
		
	}	
	
	printf("Total number of particles to be removed from all grid levels: %u \n", inboxtotal);
	
	// Reset each input file to the beginning	
	fclose(file1);
	fclose(file2);
	fclose(file3);
	fclose(file4);
	fclose(file5);
	fclose(file6);
		
	for (k = 0; k < argc - 3; k++)
	{
		// Set the correct input paths
		sprintf(inpath1, "%s", argv[k + 1]);
		sprintf(inpath2, "%s", argv[k + 1]);
		sprintf(inpath3, "%s", argv[k + 1]);
		sprintf(inpath4, "%s", argv[k + 2]);
		sprintf(inpath5, "%s", argv[k + 2]);
		sprintf(inpath6, "%s", argv[k + 2]);
	
		// Read header data from each file
		file1 = fopen(strcat(inpath1, "ic_velcx"), "r+b");
		fread(&junk, sizeof(int), 1, file1);
		fread(&np1a, sizeof(int), 1, file1);
		fread(&np2a, sizeof(int), 1, file1);
		fread(&np3a, sizeof(int), 1, file1);
		fread(&dxa, sizeof(float), 1, file1);
		fread(&x1oa, sizeof(float), 1, file1);
		fread(&x2oa, sizeof(float), 1, file1);
		fread(&x3oa, sizeof(float), 1, file1);
		fread(&astarta, sizeof(float), 1, file1);
		fread(&omegama, sizeof(float), 1, file1);
		fread(&omegava, sizeof(float), 1, file1);
		fread(&h0a, sizeof(float), 1, file1);
	
		file4 = fopen(strcat(inpath4, "ic_velcx"), "r+b");
		fread(&junk, sizeof(int), 1, file4);
		fread(&np1b, sizeof(int), 1, file4);
		fread(&np2b, sizeof(int), 1, file4);
		fread(&np3b, sizeof(int), 1, file4);
		fread(&dxb, sizeof(float), 1, file4);
		fread(&x1ob, sizeof(float), 1, file4);
		fread(&x2ob, sizeof(float), 1, file4);
		fread(&x3ob, sizeof(float), 1, file4);
		fread(&astartb, sizeof(float), 1, file4);
		fread(&omegamb, sizeof(float), 1, file4);
		fread(&omegavb, sizeof(float), 1, file4);
		fread(&h0b, sizeof(float), 1, file4);
	
		// Read header data from remaining input files
		file2 = fopen(strcat(inpath2, "ic_velcy"), "r+b");
		fread(&junk, sizeof(int), 1, file2);
		fread(&np1a, sizeof(int), 1, file2);
		fread(&np2a, sizeof(int), 1, file2);
		fread(&np3a, sizeof(int), 1, file2);
		fread(&dxa, sizeof(float), 1, file2);
		fread(&x1oa, sizeof(float), 1, file2);
		fread(&x2oa, sizeof(float), 1, file2);
		fread(&x3oa, sizeof(float), 1, file2);
		fread(&astarta, sizeof(float), 1, file2);
		fread(&omegama, sizeof(float), 1, file2);
		fread(&omegava, sizeof(float), 1, file2);
		fread(&h0a, sizeof(float), 1, file2);

		file3 = fopen(strcat(inpath3, "ic_velcz"), "r+b");
		fread(&junk, sizeof(int), 1, file3);
		fread(&np1a, sizeof(int), 1, file3);
		fread(&np2a, sizeof(int), 1, file3);
		fread(&np3a, sizeof(int), 1, file3);
		fread(&dxa, sizeof(float), 1, file3);
		fread(&x1oa, sizeof(float), 1, file3);
		fread(&x2oa, sizeof(float), 1, file3);
		fread(&x3oa, sizeof(float), 1, file3);
		fread(&astarta, sizeof(float), 1, file3);
		fread(&omegama, sizeof(float), 1, file3);
		fread(&omegava, sizeof(float), 1, file3);
		fread(&h0a, sizeof(float), 1, file3);	

		file5 = fopen(strcat(inpath5, "ic_velcy"), "r+b");
		fread(&junk, sizeof(int), 1, file5);
		fread(&np1b, sizeof(int), 1, file5);
		fread(&np2b, sizeof(int), 1, file5);
		fread(&np3b, sizeof(int), 1, file5);
		fread(&dxb, sizeof(float), 1, file5);
		fread(&x1ob, sizeof(float), 1, file5);
		fread(&x2ob, sizeof(float), 1, file5);
		fread(&x3ob, sizeof(float), 1, file5);
		fread(&astartb, sizeof(float), 1, file5);
		fread(&omegamb, sizeof(float), 1, file5);
		fread(&omegavb, sizeof(float), 1, file5);
		fread(&h0b, sizeof(float), 1, file5);

		file6 = fopen(strcat(inpath6, "ic_velcz"), "r+b");
		fread(&junk, sizeof(int), 1, file6);
		fread(&np1b, sizeof(int), 1, file6);
		fread(&np2b, sizeof(int), 1, file6);
		fread(&np3b, sizeof(int), 1, file6);
		fread(&dxb, sizeof(float), 1, file6);
		fread(&x1ob, sizeof(float), 1, file6);
		fread(&x2ob, sizeof(float), 1, file6);
		fread(&x3ob, sizeof(float), 1, file6);
		fread(&astartb, sizeof(float), 1, file6);
		fread(&omegamb, sizeof(float), 1, file6);
		fread(&omegavb, sizeof(float), 1, file6);
		fread(&h0b, sizeof(float), 1, file6);	
		
		// Finish reading header files
		fread(&junk, sizeof(int), 1, file1);
		fread(&junk, sizeof(int), 1, file1);
		fread(&junk, sizeof(int), 1, file2);
		fread(&junk, sizeof(int), 1, file2);
		fread(&junk, sizeof(int), 1, file3);
		fread(&junk, sizeof(int), 1, file3);
		fread(&junk, sizeof(int), 1, file4);
		fread(&junk, sizeof(int), 1, file4);
		fread(&junk, sizeof(int), 1, file5);
		fread(&junk, sizeof(int), 1, file5);
		fread(&junk, sizeof(int), 1, file6);
		fread(&junk, sizeof(int), 1, file6);
		
		// Check that NMULT is large enough to make file separation possible
		numfiles_out = NMULT*(argc - 2);
		printf("\n");		
		printf("Beginning processing of level %d \n", k);

		xdim = dxb*(double)np1b;
		ydim = dxb*(double)np2b;
		zdim = dxb*(double)np3b;

		for (ifile = 0; ifile < NMULT; ifile++)
		{
			// Set output path
			char current_path[50];
			sprintf(current_path, "%sIC.gad.%d", argv[argc - 1], ifile + k*NMULT);
			printf("\n");
			printf("Opening output file: %s \n", current_path);
			
			// Initialize arrays
			velx = malloc(np1a*np2a*np3a*sizeof(float)/NMULT);
			if (!velx){printf("Memory allocation failed \n");}
			vely = malloc(np1a*np2a*np3a*sizeof(float)/NMULT);
			if (!vely){printf("Memory allocation failed \n");}
			velz = malloc(np1a*np2a*np3a*sizeof(float)/NMULT);
			if (!velz){printf("Memory allocation failed \n");}

			// Initialize arrays to hold positions
			velx2 = malloc(np1a*np2a*np3a*sizeof(float)/NMULT);
			if (!velx2){printf("Memory allocation failed \n");}
			vely2 = malloc(np1a*np2a*np3a*sizeof(float)/NMULT);
			if (!vely2){printf("Memory allocation failed \n");}
			velz2 = malloc(np1a*np2a*np3a*sizeof(float)/NMULT);
			if (!velz2){printf("Memory allocation failed \n");}
				
			// Reset the number of top grid particles found within subgrid
			inbox = 0;
	
			for (i3 = 0; i3 < np3a/NMULT; i3++)
			{
				z0 = (i3 + ifile*np3a/NMULT)*dxa + x3oa;
	
				for (i2 = 0; i2 < np2a; i2++)
				{
					y0 = i2*dxa + x2oa;
	
					for (i1 = 0; i1 < np1a; i1++)
					{
						x0 = i1*dxa + x1oa;
	
						// Read velocity data from file
						fread(&velx[i1 + np2a*(i2 + np3a*i3)], sizeof(float), 1, file1);
						fread(&vely[i1 + np2a*(i2 + np3a*i3)], sizeof(float), 1, file2);
						fread(&velz[i1 + np2a*(i2 + np3a*i3)], sizeof(float), 1, file3);
							
						// Apply Zeldovich approximation to find positions
						velx[i1 + np2a*(i2 + np3a*i3)] = x0 + velx[i1 + np2a*(i2 + np3a*i3)]/vfact;
						vely[i1 + np2a*(i2 + np3a*i3)] = y0 + vely[i1 + np2a*(i2 + np3a*i3)]/vfact;	
						velz[i1 + np2a*(i2 + np3a*i3)] = z0 + velz[i1 + np2a*(i2 + np3a*i3)]/vfact;
							
						// Store positions in separate arrays
						velx2[i1 + np2a*(i2 + np3a*i3)] = velx[i1 + np2a*(i2 + np3a*i3)];
						vely2[i1 + np2a*(i2 + np3a*i3)] = vely[i1 + np2a*(i2 + np3a*i3)];	
						velz2[i1 + np2a*(i2 + np3a*i3)] = velz[i1 + np2a*(i2 + np3a*i3)];
						
						// Identify particles which lies within subgrid
						if ((velx2[i1 + np2a*(i2 + np3a*i3)] > x1ob) && (velx2[i1 + np2a*(i2 + np3a*i3)] < x1ob + xdim) &&
						    (vely2[i1 + np2a*(i2 + np3a*i3)] > x2ob) && (vely2[i1 + np2a*(i2 + np3a*i3)] < x2ob + ydim) &&
						    (velz2[i1 + np2a*(i2 + np3a*i3)] > x3ob) && (velz2[i1 + np2a*(i2 + np3a*i3)] < x3ob + zdim))
						{
							inbox = inbox + 1;
						}
					}
				}
			}
				
			printf("Removed %d particles \n", inbox);			
				
			// Calculate number of particles to be written in this file
			for (i = 0; i < 6; i++)
			{
					npart_out[i] = 0;
			}		
	
			npart_out[k + 1] = np1a*np2a*np3a/NMULT - inbox;
			vel_size = npart_out[k + 1]*3*4;
			pos_size = npart_out[k + 1]*3*4;
			id_size = npart_out[k + 1]*4;
			
			// Open output file and write header data
			current_file = fopen(current_path, "w+b");
			fwrite(&header_size, sizeof(int), 1, current_file);
			fwrite(&npart_out[0], sizeof(int), 1, current_file);
			fwrite(&npart_out[1], sizeof(int), 1, current_file);				
			fwrite(&npart_out[2], sizeof(int), 1, current_file);				
			fwrite(&npart_out[3], sizeof(int), 1, current_file);
			fwrite(&npart_out[4], sizeof(int), 1, current_file);
			fwrite(&npart_out[5], sizeof(int), 1, current_file);
			fwrite(&massarr_out[0], sizeof(double), 1, current_file);
			fwrite(&massarr_out[1], sizeof(double), 1, current_file);
			fwrite(&massarr_out[2], sizeof(double), 1, current_file);
			fwrite(&massarr_out[3], sizeof(double), 1, current_file);
			fwrite(&massarr_out[4], sizeof(double), 1, current_file);
			fwrite(&massarr_out[5], sizeof(double), 1, current_file);
			fwrite(&a_out, sizeof(double), 1, current_file);
			fwrite(&redshift_out, sizeof(double), 1, current_file);	
			fwrite(&flag_sfr_out, sizeof(int), 1, current_file);
			fwrite(&flag_feedback_out, sizeof(int), 1, current_file);
			fwrite(&nall_out[0], sizeof(int), 1, current_file);
			fwrite(&nall_out[1], sizeof(int), 1, current_file);				
			fwrite(&nall_out[2], sizeof(int), 1, current_file);				
			fwrite(&nall_out[3], sizeof(int), 1, current_file);
			fwrite(&nall_out[4], sizeof(int), 1, current_file);
			fwrite(&nall_out[5], sizeof(int), 1, current_file);				
			fwrite(&flag_cooling_out, sizeof(int), 1, current_file);
			fwrite(&numfiles_out, sizeof(int), 1, current_file);
			fwrite(&xLbox_out, sizeof(double), 1, current_file);
			fwrite(&omega0_out, sizeof(double), 1, current_file);	
			fwrite(&omegaL_out, sizeof(double), 1, current_file);
			fwrite(&hubble_out, sizeof(double), 1, current_file);
		
			for (i = 0; i < 24; i++)
			{
				fwrite(&unused_out[i], sizeof(int), 1, current_file);
			}
		
			fwrite(&header_size, sizeof(int), 1, current_file);				
				
			// Begin writing position data to file
			fwrite(&pos_size, sizeof(int), 1, current_file);
				
			for (i3 = 0; i3 < np3a/NMULT; i3++)
			{
				for (i2 = 0; i2 < np2a; i2++)
				{
					for (i1 = 0; i1 < np1a; i1++)
					{
						// Convert units to Kpc
						velx[i1 + np2a*(i2 + np3a*i3)] = velx[i1 + np2a*(i2 + np3a*i3)]*1000.0*hubble_out;
						vely[i1 + np2a*(i2 + np3a*i3)] = vely[i1 + np2a*(i2 + np3a*i3)]*1000.0*hubble_out;
						velz[i1 + np2a*(i2 + np3a*i3)] = velz[i1 + np2a*(i2 + np3a*i3)]*1000.0*hubble_out;
						
						// Omit particles which lie within the subgrid
						if ((velx2[i1 + np2a*(i2 + np3a*i3)] > x1ob) && (velx2[i1 + np2a*(i2 + np3a*i3)] < x1ob + xdim) &&
						    (vely2[i1 + np2a*(i2 + np3a*i3)] > x2ob) && (vely2[i1 + np2a*(i2 + np3a*i3)] < x2ob + ydim) &&
						    (velz2[i1 + np2a*(i2 + np3a*i3)] > x3ob) && (velz2[i1 + np2a*(i2 + np3a*i3)] < x3ob + zdim))
						{continue;}

						// Write particles which lie outside the subgrid
						fwrite(&velx[i1 + np2a*(i2 + np3a*i3)], sizeof(float), 1, current_file);
						fwrite(&vely[i1 + np2a*(i2 + np3a*i3)], sizeof(float), 1, current_file);
						fwrite(&velz[i1 + np2a*(i2 + np3a*i3)], sizeof(float), 1, current_file);
							
					}
				}
			}
	
			fwrite(&pos_size, sizeof(int), 1, current_file);
	
			// Begin writing velocity data to file
			fwrite(&vel_size, sizeof(int), 1, current_file);
			
			for (i3 = 0; i3 < np3a/NMULT; i3++)
			{
				z0 = (i3 + ifile*np3a/NMULT)*dxa + x3oa;
				
				for (i2 = 0; i2 < np2a; i2++)
				{
					y0 = i2*dxa + x2oa ;
					
					for (i1 = 0; i1 < np1a; i1++)
					{
						x0 = i1*dxa + x1oa;						
						
						// Omit particles which lie within subgrid
						if ((velx2[i1 + np2a*(i2 + np3a*i3)] > x1ob) && (velx2[i1 + np2a*(i2 + np3a*i3)] < x1ob + xdim) &&
						    (vely2[i1 + np2a*(i2 + np3a*i3)] > x2ob) && (vely2[i1 + np2a*(i2 + np3a*i3)] < x2ob + ydim) &&
						    (velz2[i1 + np2a*(i2 + np3a*i3)] > x3ob) && (velz2[i1 + np2a*(i2 + np3a*i3)] < x3ob + zdim))
						{continue;}
							
						// Write particles which lie outside subgrid
						fwrite(&velx2[i1 + np2a*(i2 + np3a*i3)], sizeof(float), 1, current_file);
						fwrite(&vely2[i1 + np2a*(i2 + np3a*i3)], sizeof(float), 1, current_file);
						fwrite(&velz2[i1 + np2a*(i2 + np3a*i3)], sizeof(float), 1, current_file);
					
					}
				}
			}

			fwrite(&pos_size, sizeof(int), 1, current_file);
				
			// Begin writing particle ids to file
			fwrite(&id_size, sizeof(int), 1, current_file);
				
			for (i = 1; i < np1a*np2a*np3a/NMULT + 1 - inbox; i++)
			{
				pid = i + prevtotal;
				fwrite(&pid, sizeof(int), 1, current_file); 
				prev = prev + 1;
			}
	
			prevtotal = prev;
			fwrite(&id_size, sizeof(int), 1, current_file);
			fclose(current_file);
		}
	
		if (k == argc - 4)
		{	
			printf("\n");
			printf("Beginning final subgrid processing \n");	
			
			for (i = 0; i < 6; i++)
			{
				npart_out[i] = 0;
			}
			
			npart_out[k + 2] = np1b*np2b*np3b/NMULT;
			vel_size = npart_out[k + 2]*3*4;
			pos_size = npart_out[k + 2]*3*4;
			id_size = npart_out[k + 2]*4;

			for (ifile=0; ifile<NMULT; ifile++)
			{
				// Initialize output file path
				char current_path[50];
				sprintf(current_path, "%sIC.gad.%d", argv[argc - 1], ifile + (k + 1)*NMULT);
				printf("\n");
				printf("Opening output file: %s \n", current_path);
			
				// Initialize arrays
				velx = malloc(np1b*np2b*np3b*sizeof(float)/NMULT);
				if (!velx){printf("Memory allocation failed \n");}
				vely = malloc(np1b*np2b*np3b*sizeof(float)/NMULT);
				if (!vely){printf("Memory allocation failed \n");}
				velz = malloc(np1b*np2b*np3b*sizeof(float)/NMULT);
				if (!velz){printf("Memory allocation failed \n");}
			
				for (i3 = 0; i3 < np3b/NMULT; i3++)
				{
					z0 = (i3 + ifile*np3b/NMULT)*dxb + x3ob;
					
					for (i2 = 0; i2 < np2b; i2++)
					{
						y0 = i2*dxb + x2ob;
						
						for (i1 = 0; i1 < np1b; i1++)
						{
							x0 = i1*dxb + x1ob;
							// Read velocities from input files
							fread(&velx[i1 + np2b*(i2 + np3b*i3)], sizeof(float), 1, file4);
							fread(&vely[i1 + np2b*(i2 + np3b*i3)], sizeof(float), 1, file5);
							fread(&velz[i1 + np2b*(i2 + np3b*i3)], sizeof(float), 1, file6);
							
							// Apply Zeldovich approximation
							velx[i1 + np2b*(i2 + np3b*i3)] = x0 + velx[i1 + np2b*(i2 + np3b*i3)]/vfact;
							vely[i1 + np2b*(i2 + np3b*i3)] = y0 + vely[i1 + np2b*(i2 + np3b*i3)]/vfact;	
							velz[i1 + np2b*(i2 + np3b*i3)] = z0 + velz[i1 + np2b*(i2 + np3b*i3)]/vfact;
	
						}
					}
				}
				
				// Open output file and write header data	
				current_file = fopen(current_path, "w+b");
				fwrite(&header_size, sizeof(int), 1, current_file);
				fwrite(&npart_out[0], sizeof(int), 1, current_file);
				fwrite(&npart_out[1], sizeof(int), 1, current_file);				
				fwrite(&npart_out[2], sizeof(int), 1, current_file);				
				fwrite(&npart_out[3], sizeof(int), 1, current_file);
				fwrite(&npart_out[4], sizeof(int), 1, current_file);
				fwrite(&npart_out[5], sizeof(int), 1, current_file);
				fwrite(&massarr_out[0], sizeof(double), 1, current_file);
				fwrite(&massarr_out[1], sizeof(double), 1, current_file);
				fwrite(&massarr_out[2], sizeof(double), 1, current_file);
				fwrite(&massarr_out[3], sizeof(double), 1, current_file);
				fwrite(&massarr_out[4], sizeof(double), 1, current_file);
				fwrite(&massarr_out[5], sizeof(double), 1, current_file);
				fwrite(&a_out, sizeof(double), 1, current_file);
				fwrite(&redshift_out, sizeof(double), 1, current_file);	
				fwrite(&flag_sfr_out, sizeof(int), 1, current_file);
				fwrite(&flag_feedback_out, sizeof(int), 1, current_file);
				fwrite(&nall_out[0], sizeof(int), 1, current_file);
				fwrite(&nall_out[1], sizeof(int), 1, current_file);				
				fwrite(&nall_out[2], sizeof(int), 1, current_file);				
				fwrite(&nall_out[3], sizeof(int), 1, current_file);
				fwrite(&nall_out[4], sizeof(int), 1, current_file);
				fwrite(&nall_out[5], sizeof(int), 1, current_file);				
				fwrite(&flag_cooling_out, sizeof(int), 1, current_file);
				fwrite(&numfiles_out, sizeof(int), 1, current_file);
				fwrite(&xLbox_out, sizeof(double), 1, current_file);
				fwrite(&omega0_out, sizeof(double), 1, current_file);	
				fwrite(&omegaL_out, sizeof(double), 1, current_file);
				fwrite(&hubble_out, sizeof(double), 1, current_file);
			
				for (i = 0; i < 24; i ++)
				{
					fwrite(&unused_out[i], sizeof(int), 1, current_file);
				}
				
				fwrite(&header_size, sizeof(int), 1, current_file);				
				
				// Begin writing position data to file
				fwrite(&pos_size, sizeof(int), 1, current_file);
				
				for (i3 = 0; i3 < np3b/NMULT; i3++)
				{
					for (i2 = 0; i2 < np2b; i2++)
					{
						for (i1 = 0; i1 < np1b; i1++)
						{
							// Convert positions to Kpc
							velx[i1 + np2b*(i2 + np3b*i3)] = velx[i1 + np2b*(i2 + np3b*i3)]*1000.0*hubble_out;
							vely[i1 + np2b*(i2 + np3b*i3)] = vely[i1 + np2b*(i2 + np3b*i3)]*1000.0*hubble_out;
							velz[i1 + np2b*(i2 + np3b*i3)] = velz[i1 + np2b*(i2 + np3b*i3)]*1000.0*hubble_out;
							
							// Write positions to file
							fwrite(&velx[i1 + np2b*(i2 + np3b*i3)], sizeof(float), 1, current_file);
							fwrite(&vely[i1 + np2b*(i2 + np3b*i3)], sizeof(float), 1, current_file);
							fwrite(&velz[i1 + np2b*(i2 + np3b*i3)], sizeof(float), 1, current_file);
							
						}
					}
				}
				
				fwrite(&pos_size, sizeof(int), 1, current_file);
	
				// Begin writing velocity data to file
				fwrite(&vel_size, sizeof(int), 1, current_file);
				
				for (i3 = 0; i3 < np3b/NMULT; i3++)
				{
					z0 = (i3 + ifile*np3b/NMULT)*dxb + x3ob;
					
					for (i2 = 0; i2 < np2b; i2++)
					{
						y0 = i2*dxb + x2ob;
						
						for (i1 = 0; i1 < np1b; i1++)
						{
							x0 = i1*dxb + x1ob;						
							
							// Undo Zeldovich approximation to return to velocities
							velx[i1 + np2b*(i2 + np3b*i3)] = (-x0 + velx[i1 + np2b*(i2 + np3b*i3)]/1000.0/hubble_out)/pow(astart,0.5)*vfact;
							vely[i1 + np2b*(i2 + np3b*i3)] = (-y0 + vely[i1 + np2b*(i2 + np3b*i3)]/1000.0/hubble_out)/pow(astart,0.5)*vfact;
							velz[i1 + np2b*(i2 + np3b*i3)] = (-z0 + velz[i1 + np2b*(i2 + np3b*i3)]/1000.0/hubble_out)/pow(astart,0.5)*vfact;

							



							// Write velocitiy data to output file							
							fwrite(&velx[i1 + np2b*(i2 + np3b*i3)], sizeof(float), 1, current_file);
							fwrite(&vely[i1 + np2b*(i2 + np3b*i3)], sizeof(float), 1, current_file);
							fwrite(&velz[i1 + np2b*(i2 + np3b*i3)], sizeof(float), 1, current_file);
			
						}
					}
				}
			
				fwrite(&vel_size, sizeof(int), 1, current_file);
				
				// Begin writing particle ids to output file
				fwrite(&id_size, sizeof(int), 1, current_file);
				
				for (i = 1; i < np1b*np2b*np3b/NMULT + 1; i++)
				{
					pid = i + prevtotal;
					fwrite(&pid, sizeof(int), 1, current_file); 
					prev = prev + 1;
				}
	
				prevtotal = prev;
				fwrite(&id_size, sizeof(int), 1, current_file);
				fclose(current_file);
			}
		}
	}	
	
	// Close input files
	fclose(file1);
	fclose(file2);
	fclose(file3);
	fclose(file4);
	fclose(file5);
	fclose(file6);
}

// Below are functions used in calculation of Zeldovich approximation
float dladt(float a, float omegam, float omegav)
{
	float dldt,eta;
	eta = pow(omegam/a + omegav*a*a + 1.0 - omegam - omegav, 0.5);
	dldt = a*eta;
	return(dldt);
}

double dplus(float a, float omegam, float omegav)
{
	double dpl, eta;
	double adp;
	
	adp = a;
	eta = pow(omegam/a + omegav*a*a + 1.0 - omegam - omegav, 0.5);
	dpl = eta/a*rombint(*ddplus, 0.0, adp, pow(10, -8), omegam, omegav);
	return(dpl);
}

float fomega(float a, float omegam, float omegav)
{
	float fom, omegak, eta;	

	if (omegam == 1.0 && omegav == 0.0)
	{
		fom = 1.0;
		return(fom);
	}
	else
	{
		omegak = 1.0 - omegam - omegav;
		eta = pow(omegam/a + omegav*a*a + omegak, 0.5);
		fom = (2.5/dplus(a, omegam, omegav) - 1.5*omegam/a - omegak)/(eta*eta);
		return(fom);
	}
}

double ddplus(float a, float omegam, float omegav)
{
	double ddp;
	double eta;	

	if (a == 0)
	{
		ddp = 0.0;
	}
	else
	{
		eta = pow(omegam/a + omegav*a*a + 1.0 - omegam - omegav, 0.5);
		ddp = 2.5 / (eta*eta*eta);
	}
	return(ddp);
}


double rombint(double (*f)(float, float, float), double a, double b, double tol, float omegam, float omegav)
{
	int maxiter = 16;
	int maxj = 5;
	double h, g0, fourj, gmax, error, g1, romb;
	double *g;
	g = malloc(sizeof(double)*(maxj + 1));
	int nint;
	int i, j, jmax, k;
	
	h = 0.5*(b - a);
	gmax = h*(f(a, omegam, omegav) + f(b, omegam, omegav));
	g[0] = gmax;
	nint = 1;
	error = pow(10,20);
	i = 0;
	while (1 == 1)
	{
		i = i + 1;
		if (i > maxiter || (i > 5 && abs(error) < tol)){break;}
		g0 = 0.0;
		for (k = 1; k < nint + 1; k++)
		{
			g0 = g0 + f(a + (k + k - 1)*h, omegam, omegav);
		}
		g0 = 0.5*g[0] + h*g0;
		h = 0.5*h;
		nint = nint + nint;
		jmax = MIN(i,maxj);
		fourj = 1.0;
		for (j = 0; j < jmax; j++)
		{
			fourj = 4.0*fourj;
			g1=g0 + (g0 - g[j])/(fourj - 1.0);
			g[j] = g0;
			g0 = g1;
		}
		if (abs(g0) > tol)
		{
			error = 1.0 - gmax/g0;
		}
		else
		{
			error = gmax;
		}
		gmax = g0;
		g[jmax] = g0;
	}
	romb = g0;	
	if (i > maxiter && abs(error) > tol)
	{
		printf("Rombint failed to converge; integral= %g, error= %g \n", romb, error);
		return(0);
	}
	return(romb);
}
